#ifndef SWARM_ALGORITHM
#define SWARM_ALGORITHM

#include <vector>
#include <string>

#include "LED.h"
#include "Bumper.h"
#include "Motor.h"
#include "robotfinder.hpp"
#include "Robot.h"
#include "SwarmMessage.h"
#define _USE_MATH_DEFINES
#include <cmath>

/*! \file
*/

/*! \class SwarmAlgorithm
 * \brief Virtual base class for a swarm algorithm
 * The SwarmAlgorithm implements the algorithm paradigm, requiring inheriting methods to implement its virtual setup and loopBody methods.  It provides inheriting algorithms attributes for algorithm-specific data, references to all the robot’s peripherals, and preconstructed action codes for use in performAction.  It also provides general algorithm utility methods.
*/
class SwarmAlgorithm {
protected:
    // action select bits    
    static const unsigned char LED_SETN = 0x08; //!< Whether or not to set an LED with this action (low-active -- sets when 0)
    static const unsigned char LED_ON  = 0x01; //!< The value setting an LED on
    static const unsigned char LED_OFF = 0x00; //!< The value setting an LED off

    static const unsigned char MOTOR_BITS    = 0x03; //!< The number of bits for a motor action
    static const unsigned char MOTOR_HOLD    = 0x03; //!< Do nothing to the motor
    static const unsigned char MOTOR_REVERSE = 0x02; //!< Reverse the motor
    static const unsigned char MOTOR_FORWARD = 0x01; //!< Advance the motor
    static const unsigned char MOTOR_STOP    = 0x00; //!< Halt the motor

    // shift amounts
    // right motor bits start at 4, after LEDs' 4
    // left are 6 after LEDs' and right motor's
    static const unsigned char MOTOR_RIGHT   = 0x04; //!< The start bit of the right motor action bits
    static const unsigned char MOTOR_LEFT    = 0x06; //!< The start bit of the left motor action bits

    //! precompiled action codes.  \see performAction
    static const unsigned char ACTION_STOP    = MOTOR_STOP    << MOTOR_LEFT | MOTOR_STOP    << MOTOR_RIGHT | LED_SETN;
    static const unsigned char ACTION_REVERSE = MOTOR_REVERSE << MOTOR_LEFT | MOTOR_REVERSE << MOTOR_RIGHT | LED_SETN;
    static const unsigned char ACTION_FORWARD = MOTOR_FORWARD << MOTOR_LEFT | MOTOR_FORWARD << MOTOR_RIGHT | LED_SETN;
    static const unsigned char ACTION_SPIN_LT = MOTOR_REVERSE << MOTOR_LEFT | MOTOR_FORWARD << MOTOR_RIGHT | LED_SETN;
    static const unsigned char ACTION_SPIN_RT = MOTOR_FORWARD << MOTOR_LEFT | MOTOR_REVERSE << MOTOR_RIGHT | LED_SETN;
    static const unsigned char ACTION_TURN_LT = MOTOR_STOP    << MOTOR_LEFT | MOTOR_FORWARD << MOTOR_RIGHT | LED_SETN;
    static const unsigned char ACTION_TURN_RT = MOTOR_FORWARD << MOTOR_LEFT | MOTOR_STOP    << MOTOR_RIGHT | LED_SETN;
    static const unsigned char ACTION_REV_LT  = MOTOR_STOP    << MOTOR_LEFT | MOTOR_REVERSE << MOTOR_RIGHT | LED_SETN;
    static const unsigned char ACTION_REV_RT  = MOTOR_REVERSE << MOTOR_LEFT | MOTOR_STOP    << MOTOR_RIGHT | LED_SETN;

    static LED *red, //!< reference to the red LED control
               *green, //!< reference to the green LED control
               *blue; //!< reference to the blue LED control
        
    static Motor *motorControl; //!< reference to the motor control
    
    static Bumper *bumperControl; //!< reference to the bumper control
    
    static Finder *robotFinder; //!< reference to the camera control

    static vector<vector<string> > * netMessages; //!< reference to a vector of incoming algorithm messages

    //! This algorithm's name
    /*! The name identifier is given to the user to run this algorithm.  When the algorithms are initialized by the SwarmBotOS, they return this identifier, which the OS in turn gives to the user’s Host as a list of each robot’s available algorithms.  While the user may select an algorithm in the Host GUI and press the Run Algorithm button, it is possible to run an algorithm directly from the Host GUI’s command line by giving the operating system command prefix (“OS:”) followed by the algorithm’s identifier (e.g., “OS:GROUPBYCOLOR”).  This is the same action for which the Run Algorithm button is a wrapper.
    */
    string id;
    
    //! Whether or not this algorithm is currently running
    /* The isRunning attribute is the algorithm’s indicator to the SwarmBotOS as to whether or not the OS should continue running the algorithm.  However, as explained in the Implementation Recommendations, this is not an indicator that the algorithm is stopped in the SwarmBotOS. Practically, no further action comes from an algorithm if isRunning is false; but to stop an algorithm, the Stop command must be given to the SwarmBotOS from the Host.  This is done by either pressing the Stop Algorithm button or sending the command (“OS:STOP”) manually via the command line.
    */
    bool isRunning;

    /*! \brief Get current algorithm messages vector
     * \fn vector<vector<string> > readMessages()
     * Returns the contents of the vector referenced by netMessages and clears that vector, which is filled by the OS each time the OS reads from the network.
     * \return the contents of the vector referenced by netMessages
     */
    vector<vector<string> > readMessages();  

    /*!  \fn string readMessageStr()
     * \brief Get the next message's string content
     * Constantly reads from the network until it receives an algorithm instruction, whose string message content it then returns.  The message is removed from the messages vector.
     * \return the next message's string content
     */
    string readMessageStr();

    /*! \fn LED *getLED()
     * Returns a reference to the LED currently lit, or null if other than exactly one LED is lit.
     * \warning may return null
     * \return a reference to the LED of the current color
     */
    LED *getLED();

    /*! \fn LED *getLED(int color)
     * Returns a reference to the LED indicated by color, or null if color is not a valid LED integer identifier as defined in LED.hpp.
     * \param color Color code of LED to get
     * \warning may return null     
     * \return a reference to the LED of the specified color
     */
    LED *getLED(int color);

    /*! \fn LED *getLED(string colorStr)
     * Returns a reference to the LED indicated by colorStr, or null if colorStr is not a valid LED string identifier as defined in LED.hpp.
     * \param colorStr Color string of LED to get
     * \warning may return null     
     * \return a reference to the LED of the specified color
     */
    LED *getLED(string colorStr);
    
    /*! \fn static int getClosestRobotIndex(vector<Robot> robots, int color = LED::ANY, bool forward = true)
     * \brief Gets the closest robot
     * Requires a vector of Robots as given by Finder.getRobotPositions(), a color of robot to seek defaulting to any color, and whether or not to exclude search to the front of the robot, defaulting to searching to the front.  Returns an index into the Robots vector, indexing the closest robot given color and range criteria.
     * \param robots robot vector from Finder.getRobotPositions.
     * \param color LED color code to search for (default LED::ANY)
     * \param forward whether or not to restrict search to forward of robot
     * \return index into robots of closest specified robot
    */
    static int getClosestRobotIndex(vector<Robot> robots, int color = LED::ANY, bool forward = true);

    /*! \fn static int getFurthestRobotIndex(vector<Robot> robots, int color = LED::ANY)
     * \brief Gets the furthest robot
     * Requires a vector of Robots as given by Finder.getRobotPositions() and a color of robot to seek defaulting to any color.  Returns an index into the Robots vector, indexing the furthest robot given color criteria.
     * \param robots robot vector from Finder.getRobotPositions.
     * \param color LED color code to search for (default LED::ANY)
     * \return index into robots of furthest specified robot
    */
    static int getFurthestRobotIndex(vector<Robot> robots, int color = LED::ANY);

    /*! \fn static int getBumperRobotIndex(vector<Robot> robots, bool isFront = true, double offset = M_PI_4, double furthestTouch = 17.0)
     * \brief Gets the closest robot at bumper
     * Requires a vector of Robots as given by Finder.getRobotPositions(); whether the bumper in question is the front or back, defaulting to front; the angle offset from center within which another robot registers as bumping, defaulting to π/4; and a distance beyond which another robot does not register as bumping, defaulting to an estimated 17 cm.  Returns an index into the Robots vector, indexing the closest robot off of the specified bumper, closer than furthestTouch, and within the given angle offset.
     * \param robots robot vector from Finder.getRobotPositions.
     * \param isFront whether the bumper in question is the front (not back)
     * \param offset angle offset from front in radians within which to search
     * \param furthestTouch radius within which to search
     * \return index into robots of closest specified robot
    */
    static int getBumperRobotIndex(vector<Robot> robots, bool isFront = true, double offset = M_PI_4, double furthestTouch = 17.0);

    /*! \fn unsigned char getRandomMotorAction()
     * \brief Generate random motor action code
     * Generates a action code that encodes either a forward, stop, or reverse action on both servos.
     * \return action code for random motor action
     */
    unsigned char getRandomMotorAction();

    /*! \fn void performAction(unsigned char action, unsigned char rightOffset, unsigned char leftOffset)
     * \brief Perform an action code
     * Parses and executes the given action code.  Speeds offset from neutral must be given for the right and left servos (see Motor object documentation for explanation of speed offsets).
     * \param action action code to perform
     * \param rightOffset offset from right servo neutral speed at which to run right servo
     * \param leftOffset offset from left servo neutral speed at which to run left servo
     */
    void performAction(unsigned char action, unsigned char rightOffset, unsigned char leftOffset);

public:
    /*! \fn static void loadControls(LED *r, LED *g, LED *b, Motor *mot, Bumper *bump, Finder *finder, vector<vector<string> > *ms)
     * \brief load peripheral controls
     * Loads the static references to the peripherals given -- LEDs, motor, and bumper controls; camera Finder; and algorithm message vector.  Since these references are static, this is only run once by the OS.
     */
    static void loadControls(LED *r, LED *g, LED *b, Motor *mot, Bumper *bump, Finder *finder, vector<vector<string> > *ms);

    /*! \fn static void turnLEDsOff()
     * \brief turn LEDs off
     * Simply turns all the LEDs off.
     */
    static void turnLEDsOff();

    /*! \fn SwarmAlgorithm()
     * Basic constructor.  Ran by inheriting algorithm objects when OS constructs them.
     */
    SwarmAlgorithm();

    string getID();
   
    LED *getRed();
    LED *getGreen();
    LED *getBlue();

    Motor *getMotorControl();
    
    Bumper *getBumperControl();
    
    Finder *getRobotFinder();

    bool getIsRunning();

    /*! \fn virtual bool setup() = 0
     * specifies algorithm setup
     */
    virtual bool setup() = 0;
    
    /*! \fn virtual void loopBody() = 0
     * specifies algorithm execution frame
    */
    virtual void loopBody() = 0;
};

#endif
